#include <stdio.h>
#include <math.h>
#include "SampleLoopTask.h"
#include <string.h>

#include "main.h"
#include "motor.h"
#include "macros.h"

#define M_PER_VOLT		(0.03333)

double num[50];
char signame[50];


SampleLoopTask::SampleLoopTask() : PeriodicTask(){

}

SampleLoopTask::~SampleLoopTask(){

}

int SampleLoopTask::Init(char *name, double rate, double *actual_sample_rate, int priority){

	PeriodicTask::Init(name, rate, actual_sample_rate, priority);

	SampleRate = *actual_sample_rate;
	
	printf("actual sample rate %lf\n", *actual_sample_rate);
	
	for(int i=0; i<4; i++){
		MNET->AddSignal(i, &(num[i]));
	}
	
	// initialize gains
	Kp = 300.;
	Kd = 10.;
	Ki = 0.;
	
	pCmd = 0.;
	
	done = 0;
	
	ncycles_prev = 0;
	cps = SYSPAGE_ENTRY(qtime)->cycles_per_sec;
	
	lp.go = 0;

}


int SampleLoopTask::TimingStart(double sec){
	
	lp.mu_prev = 1./SampleRate * 1.e6;
	lp.s_sq_prev = 0.;
	lp.std = 0.;
	
	lp.min = lp.max = 1./SampleRate * 1.e6;
	
	printf("%lf %lf\n", lp.min, lp.max);
	
	sem_init(&lp.sem, 1, 0);
	
	lp.numSamples = (int)(sec * SampleRate);
	lp.i = 1;
	lp.maxIndex = -1;
	lp.minIndex = -1;
	
	lp.go = 1;
	
	// block
	sem_wait(&lp.sem);
	
	printf("mean %lf us\n", lp.mu);
	printf("std %lf us\n", lp.std);
	printf("max %lf us at %d\n", lp.max, lp.maxIndex);
	printf("min %lf us at %d\n", lp.min, lp.minIndex);
	
	sem_destroy(&lp.sem);
}

int SampleLoopTask::TimingProcess(){

	if(lp.go && lp.i <= lp.numSamples){
		// get time
		lp.t = sec * 1.e6; // microseconds
		
		if(lp.t > lp.max){
			lp.max = lp.t;
			lp.maxIndex = lp.i;
		}
		
		if(lp.t < lp.min){
			lp.min = lp.t;
			lp.minIndex = lp.i;
		}
		
		lp.mu = lp.mu_prev + (lp.t - lp.mu_prev) / (((double)lp.i) + 1.);
		lp.s_sq = (1. - 1./((double)lp.i)) * lp.s_sq_prev + (((double)lp.i) + 1.) * (lp.mu - lp.mu_prev) * (lp.mu - lp.mu_prev);
		
		lp.mu_prev = lp.mu;
		lp.s_sq_prev = lp.s_sq;
		
		if(lp.i == lp.numSamples){
			lp.std = sqrt(lp.s_sq);
			lp.go = 0;
			// unblock;
			sem_post(&lp.sem);
			
		}
		
		lp.i++;
	}
}


void SampleLoopTask::Task(){
	int cnt;
	int region;
	int attempt;
	
	static int dout = 1;
	static int last_status = 0;
	static double pos_prev = 0;
	static double pCmd_prev = 0;
	static double vCmd_prev = 0;
	static double error = 0;
	
	int temp = 0;

	while(!lost){
		TimerWait();
		
		ncycles = ClockCycles();
		sec=(double)(ncycles - ncycles_prev)/cps;
		ncycles_prev = ncycles;
		
		TimingProcess();
		
		// Read Inputs
		HW->ProcessInput();
		
		// Get status of camera
		last_status = HW->ReadDigitalBit(IoHardware::FRAME_STATUS);
		
		// Send out pulse to trigger camera
		HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 1);
		HW->WriteDigitalBit(IoHardware::CAMERA_TRIGGER, 0);
		
		// square wave command
		/*if(++cnt % (int)(SAMPLE_RATE) == 0){
			dout = !dout;
			
			if(dout){
				pCmd = 0.785398;
			}
			else{
				pCmd = -0.785398;
			}
			
			//HW->SetDigitalOutBit(IoHardware::DOUT_48, dout);
			//printf("Input: %d\n", HW->GetDigitalInBit(IoHardware::DIN_71));
			//printf("Potentiometer: %f\n", HW->GetAnalogIn(IoHardware::AIN_0));
			//printf("Error: %f\n", error);
		}*/
		
		// sinusoid command
		pCmd = 0.785398*sin(2*3.14159*(++cnt)/SAMPLE_RATE);
		
		// controller
		enc = -(HW->GetEncoderCount(IoHardware::ENC_0)); // sign change to agree with camera
		
		pos = (enc * ENC_RAD_PER_CNT) / GR / 4; // convert to rad
		vel = (pos - pos_prev) * SAMPLE_RATE;
		error += (pos - pCmd);
		
		vCmd = (pCmd - pCmd_prev) * SAMPLE_RATE;
		aCmd = (vCmd - vCmd_prev) * SAMPLE_RATE;
		
		pos_prev = pos;
		pCmd_prev = pCmd;
		vCmd_prev = vCmd;
		
		iCmd = calcI(vCmd, aCmd); // feedforward control
		
		iCmd += (Kp * (pCmd - pos) - Kd * vel - Ki * error); // feedback control
		
		// limit current based on motor specs
		if(iCmd > MAX_CURRENT_MA){
			iCmd = MAX_CURRENT_MA;
		}
		else if(iCmd < -MAX_CURRENT_MA){
			iCmd = -MAX_CURRENT_MA;
		}
		
		ampVCmd = -iCmd * AMP_GAIN; // convert to analog signal
		// sign change to agree with camera
		
		// output signal to amp
		if(!done){
			//HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, ampVCmd);
			HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
		}
		else{
			HW->WriteAnalogCh(IoHardware::AMP_SIGNAL, 0.0);
		}
		
		/*num[0] = pCmd;
		num[1] = pos;
		num[2] = 0.;
		num[3] = ampVCmd;*/
		
		HW->ProcessOutput();
		
		if(camera && !lost){
			// Wait for camera to process data, with timeout counter
			while(HW->ReadDigitalBit(IoHardware::FRAME_STATUS) == last_status){
				if(++attempt == (int)(1e8/SAMPLE_RATE) FATAL_ERROR("Frame not received -");
			}
			attempt = 0;
			
			// Read in analog voltages to get position/orientation data
			region = (int)round(HW->ReadAnalogCh(IoHardware::ROI));
			switch(region){
				case ROI_0:
					obj.x0 = HW->ReadAnalogCh(IoHardware::OBJ_X)*M_PER_VOLT;
					obj.y0 = HW->ReadAnalogCh(IoHardware::OBJ_Y)*M_PER_VOLT;
					break;
				case ROI_1:
					obj.x1 = HW->ReadAnalogCh(IoHardware::OBJ_X)*M_PER_VOLT;
					obj.y1 = HW->ReadAnalogCh(IoHardware::OBJ_Y)*M_PER_VOLT;
					break;
				default:
					FATAL_ERROR("Object lost -");
					lost = 1;
					break;
			}
			
			obj.x = (obj.x0 + obj.x1)/2;
			obj.y = (obj.y0 + obj.y1)/2;
			obj.theta = atan2(obj.y1-obj.y0, obj.x1-obj.x0);
		}
		
		// set outputs
		num[0] = pos; // angle in rad
		num[1] = obj.theta;
		num[2] = obj.x1; // position in m
		num[3] = obj.y1;
		
		// Send Outputs to MATLAB
		MNET->Process();
	}
}

